/// 通过ceres构建求解相机参数  3个旋转角度 + 3个平移 + f + u0,v0
/// Cr_RigidTransform.cpp 中可以求出 两个坐标之间的转化矩阵
/// TODO: 这里构造了好几个点，都无法求出 f, u, v
///
#include "ceres/ceres.h"
#include "glog/logging.h"

#include <memath/meTransform.h>
#include <memath/MeEigenMath.h>

using namespace std;

struct ProjMCostFunction {
    ProjMCostFunction(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2):
        pI(p1), pO(p2){}
    template <typename T>
    bool operator()(const T* const parameters,
                    T* residuals) const {
        const T& x = parameters[0];
        const T& y = parameters[1];
        const T& z = parameters[2];

        const T& t1 = parameters[3];
        const T& t2 = parameters[4];
        const T& t3 = parameters[5];

        const T& f = parameters[6];
        const T& u = parameters[7];
        const T& v = parameters[8];
        //double u = 0, v=0;

        T px = cos(y)*cos(z)*pI[0] + (sin(x)*sin(y)*cos(z) - cos(x)*sin(z))*pI[1] + (cos(x)*sin(y)*cos(z) + sin(x)*sin(z))*pI[2] + t1;
        T py = cos(y)*sin(z)*pI[0] + (sin(x)*sin(y)*sin(z) + cos(x)*cos(z))*pI[1] + (cos(x)*sin(y)*sin(z) - sin(x)*cos(z))*pI[2] + t2;
        T pz = -sin(y)*pI[0] + (sin(x)*cos(y))*pI[1] + (cos(x)*cos(y))*pI[2] + t3;

//        residuals[0] = px*f/pz + u - pO[0];
//        residuals[1] = py*f/pz + v - pO[1];
        residuals[0] = px*f + u *pz - pO[0]*pz;
        residuals[1] = py*f + v *pz - pO[1]*pz;
        // residuals[2] = pz - pO[2];

        return true;
    }

    const Eigen::Vector3d pI;
    const Eigen::Vector3d pO;
};


int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);

    int option = 0;
    if (argc > 1) {
        option = atoi(argv[0]);
    }

    // 生成数据
    Matrix4d m = MeTransform::rotateZ(30*ME_DEGREE2RAD)
            *MeTransform::rotateY(20*ME_DEGREE2RAD)
            *MeTransform::rotateX(10*ME_DEGREE2RAD);

    m(0, 3) = 1;
    m(1, 3) = 2;
    m(2, 3) = 300;
    double f = 1300;
    double u = 400, v=400;

    vector<Eigen::Vector3d> p_I = {
        {33.50, 73.77, 78},
        {-3.27, 37, 78},
        {33.50, 0.23, 78},
        {70.27, 37, 78},
        {33.50, 63.77, 78},
        // plane2
        {67, 7, 2},
        {0, 0, 2},
        {0, 74, 2},
        {67, 67, 2},
        {71.95, 2.05, 2},
    };

    vector<Eigen::Vector3d> p_O;
    for(uint i=0; i< p_I.size(); i++) {
        Eigen::Vector3d pO = MeEigenMath::multiplyVector3d(m, p_I[i]);
        pO[0] = pO[0]*f/pO[2] + u;
        pO[1] = pO[1]*f/pO[2] + v;
        pO[2] = 1;
//        pO[0] = pO[0]*f;
//        pO[1] = pO[1]*f;
        p_O.push_back(pO);
    }


    // Build the problem.
    ceres::Problem problem;

    double parameters[9] = {1*ME_DEGREE2RAD, 10*ME_DEGREE2RAD, 0*ME_DEGREE2RAD,
                           0, 0, 100,
                            100, 0, 0};

    // Set up the only cost function (also known as residual).
    for (uint i=0; i<p_I.size();i++) {
        ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<ProjMCostFunction, 2, 9>(
                new ProjMCostFunction(p_I[i], p_O[i]));
        problem.AddResidualBlock(cost_function, NULL, parameters);
    }

    // Run the solver!
    ceres::Solver::Options options;
    options.minimizer_progress_to_stdout = true;
    options.gradient_tolerance = 1e-16;
    options.function_tolerance = 1e-16;
    options.parameter_tolerance = 1e-16;
    options.linear_solver_type = ceres::DENSE_QR;
    ceres::Solver::Summary summary;
    Solve(options, &problem, &summary);

    //std::cout << summary.BriefReport() << "\n";
    std::cout << summary.FullReport() << "\n";

    std::cout << "Expect   x: " << 10*ME_DEGREE2RAD << " y: " << 20*ME_DEGREE2RAD << " z: " << 30*ME_DEGREE2RAD
              << "\n";
    std::cout << "Final   x: " << parameters[0] << " y: " << parameters[1] << " z: " << parameters[2]
              << " t1: " << parameters[3] << " t2: " << parameters[4] << " t3: " << parameters[5]
              << " f: " << parameters[6]  << " u: " << parameters[7]  << " v: " << parameters[8]
              << "\n";

    Matrix4d m2 = MeTransform::rotateZ(parameters[2])
            *MeTransform::rotateY(parameters[1])
            *MeTransform::rotateX(parameters[0]);
    m2(0, 3) = parameters[3];
    m2(1, 3) = parameters[4];
    m2(2, 3) = parameters[5];

    cout << "m:\n" << m << endl;
    cout << "m2:\n" << m2 << endl;
    return 0;
}
